import socket
import struct
import numpy as np

# 设置目标主机和端口
HOST = '0.0.0.0'  # 监听所有可用的接口
PORT = 7788

def quaternion_to_rotation_matrix(q):
    q_x, q_y, q_z, q_w = q
    
    # 计算旋转矩阵
    R = np.array([
        [1 - 2*(q_y**2 + q_z**2), 2*(q_x*q_y - q_w*q_z), 2*(q_x*q_z + q_w*q_y)],
        [2*(q_x*q_y + q_w*q_z), 1 - 2*(q_x**2 + q_z**2), 2*(q_y*q_z - q_w*q_x)],
        [2*(q_x*q_z - q_w*q_y), 2*(q_y*q_z + q_w*q_x), 1 - 2*(q_x**2 + q_y**2)]
    ])
    
    return R

# 创建 UDP 套接字
with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as s:
    # 绑定套接字到地址
    s.bind((HOST, PORT))
    
    print(f"正在监听 {HOST}:{PORT} ...")
    
    # while True:
    # 接收数据
    data, addr = s.recvfrom(1248)  # 接收 1248 字节的数据包
    if len(data) != 1248:
        print("接收到的数据包长度不正确。")
        # continue
    
    # 提取 IMU 标定数据
    imu_data_start = 1092
    imu_data_end = 1092 +28 # 1092 是不包含的索引
    imu_data = data[imu_data_start:imu_data_end]
    
    imu_data = data[imu_data_start:imu_data_end]
        
    # 进行转换
    # 使用 struct.unpack 解析 IMU 数据
    q_x, q_y, q_z, q_w, x, y, z = struct.unpack('>fffffff', imu_data)
    quaternion = (q_x, q_y, q_z, q_w)  # 使用从 IMU 数据中提取的四元数

    # 给定的四元数
    q = (-0.005345283541828394, -0.7052773237228394, 0.7088991403579712, -0.004164167679846287)
    R = quaternion_to_rotation_matrix(q)
    
    # 打印 IMU 数据
    print(f"序号: {imu_data[0]}")
    print(f"q_x: {q_x}, q_y: {q_y}, q_z: {q_z}, q_w: {q_w}")
    print(f"x: {x}, y: {y}, z: {z}")
    print("旋转矩阵:")
    print(R)

